﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FarseerPhysics.Dynamics;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
using FarseerPhysics.Common;
using FarseerPhysics.Factories;
using Microsoft.Xna.Framework.Input;
using FarseerPhysics.Dynamics.Contacts;
using Microsoft.Xna.Framework.Graphics;

namespace FarseerPhysics.HelloWorld
{
    class CpuBody : IMyBody
    {            
        private World _world;
        private Vertices _pathToFollow;
        private Body _bodyTail;
        private RevoluteJoint _rjoint;
        private SpriteFont _font;

        const float DEGTORAD = 0.0174532925f;
        private int _current = 0;
        const int kMaxPath = 25;
        //bike parameters
        const float MaxSteerAngle = Settings.Pi / 3;
        const float MaxKillValue = 1.0f; // max killing othogonal velocity
        const float SteerSpeed = 1.0f;  // steer speed  
        const float TorqeSpeed = 0.004f;  // steer speed  /0,03
        const float Horsepower = 8.0f;   // max speed
        const float acc = 0.09f;         //determines acceleration of a car
        const float dcc = 0.6f;         // decceleration of car
        const float kLinearDamping = 0.8f;
        const float kAngularDamping = 3.8f;

        Vector2 _carStartingPosition = new Vector2(12f, 19f);
        Vector2 _startingPosition = new Vector2(12f, 25f);

        float _engineSpeed;
        float _steeringAngle;
        //float _killValue = 1.0f;
        public int _bodyType1 { get; set; }
        public int _bodyType2 { get; set; }

        public Body _body { get; set; }
        private CFileLogger _logger = new CFileLogger("CpuBody.log");

        public CpuBody(World world,Vertices path)
        {
            _world = world;
            _pathToFollow = path;
        }

        public override void LoadContent()
        {
           
            /*            
           _body = new Body(_world);        
           // define our shapes
           PolygonShape box = new PolygonShape(1);
           box.SetAsBox( 0.9f, 0.45f );  //offset from front and rear of the car(for proper wheel location)            
           _body.CreateFixture(box);
           _body.BodyType = BodyType.Dynamic;
           _body.LinearDamping = kLinearDamping;
           _body.AngularDamping = kAngularDamping;
           _body.Position = _carStartingPosition;
           _body.Rotation = MathHelper.ToRadians(90);
           _body.Mass = 1;          
            */

            Vertices rect1 = PolygonTools.CreateRectangle(0.2f, 0.02f);
            Vertices rect2 = PolygonTools.CreateRectangle(0.02f, 0.07f);

            Vector2 trans = new Vector2(-0f, 0f);
            rect1.Translate(ref trans);
            trans = new Vector2(0f, -0.2f);
            rect2.Translate(ref trans);

            List<Vertices> vertices = new List<Vertices>(2);
            vertices.Add(rect1);
            vertices.Add(rect2);

            _body = BodyFactory.CreateCompoundPolygon(_world, vertices, 1f);
            _body.BodyType = BodyType.Dynamic;
            _body.LinearDamping = kLinearDamping;
            _body.AngularDamping = kAngularDamping;
            _body.Position = _carStartingPosition;
            _body.Rotation = MathHelper.ToRadians(90);
            _body.Mass = 1;

            _bodyTail = BodyFactory.CreateRectangle(_world, 0.05f, 0.45f, 1f);
            _bodyTail.Position = _startingPosition;
            _bodyTail.BodyType = BodyType.Dynamic;
            _bodyTail.LinearDamping = kLinearDamping;
            _bodyTail.AngularDamping = kAngularDamping;
            _bodyTail.Position = _carStartingPosition + (new Vector2(0f, 0.2f));
            _bodyTail.Rotation = MathHelper.ToRadians(90);
            _bodyTail.Mass = 0.01f;

            _rjoint = JointFactory.CreateRevoluteJoint(_body, _bodyTail, Vector2.Zero);
            _rjoint.LowerLimit = MathHelper.ToRadians(-45);
            _rjoint.UpperLimit = MathHelper.ToRadians(45);
            _rjoint.LimitEnabled = true;
            _rjoint.LocalAnchorA = new Vector2(0.0f, 0.0f);
            _rjoint.LocalAnchorB = new Vector2(0.0f, -0.2f);
            _rjoint.MotorEnabled = true;
            _rjoint.MaxMotorTorque = 100f;

            _world.AddJoint(_rjoint);
            _body.OnCollision += MyOnCollision;
        }

        public override void Update()
        {
            //Driving            
            Transform tmp;
            _body.GetTransform(out tmp);
            Vector2 ldirection = tmp.R.Col2 * _engineSpeed;
            _body.ApplyForce(ldirection);

           
            Transform tmp2;
            _bodyTail.GetTransform(out tmp2);
            Vector2 rdirection = tmp.R.Col2 * _engineSpeed * 0.01f;
            _bodyTail.ApplyForce(rdirection);
           
            //Steering
            
            _body.ApplyTorque(_steeringAngle * TorqeSpeed);
          //  _rjoint.MotorSpeed = _steeringAngle;
/*
            float mspeed = _steeringAngle - _rjoint.JointAngle;
            _rjoint.MotorSpeed = mspeed * SteerSpeed;          
 */
            
        }

        public override void HandleKeyboard()
        {
            KeyboardState state = Keyboard.GetState();    

            //_engineSpeed = MathHelper.SmoothStep(_engineSpeed, -Horsepower, acc);
            _engineSpeed = -2f;
            
            if (_current > kMaxPath)
                _current = 0;

            float bodyAngle = _body.Rotation;
            Vector2 toTarget = _pathToFollow[_current] - _body.Position;
            if (toTarget.Length() < 1)
                _engineSpeed = toTarget.Length();
                        
            float desiredAngle = (float)Math.Atan2(toTarget.Y, toTarget.X);
//            float desAngDeg = MathHelper.ToDegrees(desiredAngle);
            //float desiredAngleDeg = MathHelper.ToDegrees((float)Math.Atan2(toTarget.Y, (float)toTarget.X));

            _body.SetTransform(_body.Position, MathHelper.ToDegrees(desiredAngle));

           // _logger.Log(this, MathHelper.ToDegrees(desiredAngle).ToString());
            //_logger.Log(this, toTarget.Length().ToString());
            /*
            float totalRotation = desiredAngle - bodyAngle;
            //_logger.Log(this, MathHelper.ToDegrees(totalRotation.ToString());
            //while ( totalRotation < -180 * DEGTORAD ) totalRotation += 360 * DEGTORAD;
            //while ( totalRotation >  180 * DEGTORAD ) totalRotation -= 360 * DEGTORAD;
            
            if ( totalRotation < 0 ) 
                _steeringAngle = MaxSteerAngle;
            else if ( totalRotation > 0 )
                _steeringAngle = -MaxSteerAngle;
            else                
              _steeringAngle = 0;
           */
            if (state.IsKeyDown(Keys.End))
            {
                _body.Position = _carStartingPosition;
                _body.Rotation = MathHelper.ToRadians(90);
            }       
        }

        public override void Draw(SpriteBatch spriteBatch)
        {
           
        }
        private bool MyOnCollision(Fixture f1, Fixture f2, Contact contact)
        {
            //We want the collision to happen, so we return true.
            _bodyType1 = f1.Body.BodyId;
            _bodyType2 = f2.Body.BodyId;
            return true;
        }    
    
    }
}
